% 读取并可视化四足机器人关节扭矩-速度点云数据（优化版）
% 作者: Chen Qiabang
% 日期: 2025-10-14
% 版本: 2.0
% 用途: 读取从Python工具导出的CSV点云数据并进行高级可视化分析

function read_point_cloud_data(csv_filename)
    %% 参数设置
    % 如果没有提供文件名，弹出文件选择对话框
    if nargin < 1
        [filename, pathname] = uigetfile('*.csv', '选择点云数据CSV文件');
        if isequal(filename, 0)
            disp('用户取消了文件选择');
            return;
        end
        csv_filename = fullfile(pathname, filename);
    end
    
    %% 数据加载
    fprintf('========================================\n');
    fprintf('四足机器人关节点云数据分析工具 v2.0\n');
    fprintf('========================================\n\n');
    fprintf('正在读取文件: %s\n', csv_filename);
    
    try
        % 读取CSV文件
        opts = detectImportOptions(csv_filename);
        opts.DataLine = 6;  % 从第6行开始读取数据
        opts.VariableNamesLine = 5;  % 第5行是列名
        data = readtable(csv_filename, opts);
        
        % 显示加载信息
        fprintf('✓ 数据加载成功！\n');
        fprintf('  总数据点数: %d\n', height(data));
        fprintf('  时间范围: %s 至 %s\n', string(data.Timestamp(1)), string(data.Timestamp(end)));
        
    catch ME
        fprintf('❌ 读取文件失败: %s\n', ME.message);
        return;
    end
    
    %% 数据提取和验证
    timestamps = data.Timestamp;
    leg_ids = data.Leg_ID;
    motor_ids = data.Motor_ID;
    torques = data.Torque_Nm_;
    speeds = data.Speed_rad_s_;
    
    % 数据质量检查
    fprintf('\n数据质量检查:\n');
    num_nan_torque = sum(isnan(torques));
    num_nan_speed = sum(isnan(speeds));
    fprintf('  NaN值 - 扭矩: %d, 速度: %d\n', num_nan_torque, num_nan_speed);
    
    % 移除NaN值
    valid_idx = ~isnan(torques) & ~isnan(speeds);
    if sum(~valid_idx) > 0
        fprintf('  移除 %d 个无效数据点\n', sum(~valid_idx));
        torques = torques(valid_idx);
        speeds = speeds(valid_idx);
        leg_ids = leg_ids(valid_idx);
        motor_ids = motor_ids(valid_idx);
    end
    
    % 配置
    leg_names = {'Leg 0', 'Leg 1', 'Leg 2', 'Leg 3'};
    motor_names = {'Motor 1', 'Motor 2', 'Motor 3', 'Motor 4'};
    leg_colors = [0 0.4470 0.7410;  % 蓝色
                  0.8500 0.3250 0.0980;  % 橙色
                  0.9290 0.6940 0.1250;  % 黄色
                  0.4940 0.1840 0.5560]; % 紫色
    
    %% 可视化1: 按腿部分组（增强版）
    fig1 = figure('Name', '点云图 - 按腿部视图', 'Position', [50, 50, 1600, 1000]);
    sgtitle('扭矩 vs 速度点云图 - 按腿部视图', 'FontSize', 16, 'FontWeight', 'bold');
    
    for leg_id = 0:3
        for motor_id = 0:3
            % 筛选数据
            mask = (leg_ids == leg_id) & (motor_ids == motor_id);
            leg_torques = torques(mask);
            leg_speeds = speeds(mask);
            
            if isempty(leg_torques)
                continue;
            end
            
            % 创建子图
            subplot(4, 4, leg_id*4 + motor_id + 1);
            scatter(leg_speeds, leg_torques, 2, leg_colors(leg_id+1,:), ...
                    'filled', 'MarkerFaceAlpha', 0.4);
            
            xlabel('速度 (rad/s)', 'FontSize', 9);
            ylabel('扭矩 (Nm)', 'FontSize', 9);
            title(sprintf('%s - %s (n=%d)', leg_names{leg_id+1}, ...
                  motor_names{motor_id+1}, length(leg_torques)), ...
                  'FontSize', 10, 'FontWeight', 'bold');
            grid on;
            axis tight;
            
            % 添加统计信息
            text(0.05, 0.95, sprintf('μ_t=%.2f\nσ_t=%.2f', ...
                 mean(leg_torques), std(leg_torques)), ...
                 'Units', 'normalized', 'FontSize', 7, ...
                 'BackgroundColor', 'white', 'EdgeColor', 'black', ...
                 'VerticalAlignment', 'top');
        end
    end
    
    %% 可视化2: 按电机分组（增强版）
    fig2 = figure('Name', '点云图 - 按电机视图', 'Position', [100, 100, 1600, 1000]);
    sgtitle('扭矩 vs 速度点云图 - 按电机视图', 'FontSize', 16, 'FontWeight', 'bold');
    
    for motor_id = 0:3
        for leg_id = 0:3
            % 筛选数据
            mask = (motor_ids == motor_id) & (leg_ids == leg_id);
            motor_torques = torques(mask);
            motor_speeds = speeds(mask);
            
            if isempty(motor_torques)
                continue;
            end
            
            % 创建子图
            subplot(4, 4, motor_id*4 + leg_id + 1);
            scatter(motor_speeds, motor_torques, 2, leg_colors(leg_id+1,:), ...
                    'filled', 'MarkerFaceAlpha', 0.4);
            
            xlabel('速度 (rad/s)', 'FontSize', 9);
            ylabel('扭矩 (Nm)', 'FontSize', 9);
            title(sprintf('%s - %s (n=%d)', motor_names{motor_id+1}, ...
                  leg_names{leg_id+1}, length(motor_torques)), ...
                  'FontSize', 10, 'FontWeight', 'bold');
            grid on;
            axis tight;
            
            % 添加统计信息
            text(0.05, 0.95, sprintf('μ_v=%.2f\nσ_v=%.2f', ...
                 mean(motor_speeds), std(motor_speeds)), ...
                 'Units', 'normalized', 'FontSize', 7, ...
                 'BackgroundColor', 'white', 'EdgeColor', 'black', ...
                 'VerticalAlignment', 'top');
        end
    end
    
    %% 可视化3: 所有数据叠加（增强版）
    fig3 = figure('Name', '点云图 - 全部数据叠加', 'Position', [150, 150, 1000, 700]);
    hold on;
    
    for leg_id = 0:3
        mask = (leg_ids == leg_id);
        leg_torques = torques(mask);
        leg_speeds = speeds(mask);
        
        scatter(leg_speeds, leg_torques, 3, leg_colors(leg_id+1,:), ...
                'filled', 'MarkerFaceAlpha', 0.3, ...
                'DisplayName', sprintf('%s (n=%d)', leg_names{leg_id+1}, sum(mask)));
    end
    
    xlabel('速度 (rad/s)', 'FontSize', 13, 'FontWeight', 'bold');
    ylabel('扭矩 (Nm)', 'FontSize', 13, 'FontWeight', 'bold');
    title('扭矩 vs 速度点云图 - 所有关节', 'FontSize', 15, 'FontWeight', 'bold');
    legend('Location', 'best', 'FontSize', 10);
    grid on;
    axis tight;
    hold off;
    
    %% 可视化4: 密度热力图（新增）
    fig4 = figure('Name', '密度热力图', 'Position', [200, 200, 1400, 500]);
    
    for leg_id = 0:3
        subplot(1, 4, leg_id + 1);
        mask = (leg_ids == leg_id);
        leg_torques = torques(mask);
        leg_speeds = speeds(mask);
        
        if ~isempty(leg_torques)
            % 创建2D直方图（密度图）
            [N, Xedges, Yedges] = histcounts2(leg_speeds, leg_torques, 50);
            imagesc(Xedges, Yedges, N');
            axis xy;
            colorbar;
            colormap(hot);
            
            xlabel('速度 (rad/s)', 'FontSize', 10);
            ylabel('扭矩 (Nm)', 'FontSize', 10);
            title(sprintf('%s 密度分布', leg_names{leg_id+1}), ...
                  'FontSize', 11, 'FontWeight', 'bold');
        end
    end
    sgtitle('扭矩-速度密度热力图', 'FontSize', 14, 'FontWeight', 'bold');
    
    %% 可视化5: 相关性分析（新增）
    fig5 = figure('Name', '相关性分析', 'Position', [250, 250, 1200, 800]);
    
    corr_data = zeros(4, 4);
    for leg_id = 0:3
        for motor_id = 0:3
            mask = (leg_ids == leg_id) & (motor_ids == motor_id);
            if sum(mask) > 1
                leg_torques = torques(mask);
                leg_speeds = speeds(mask);
                corr_data(leg_id+1, motor_id+1) = corr(leg_speeds, leg_torques);
            end
        end
    end
    
    % 绘制相关系数热力图
    subplot(2, 2, [1 2]);
    imagesc(corr_data);
    colorbar;
    colormap(jet);
    caxis([-1 1]);
    
    set(gca, 'XTick', 1:4, 'XTickLabel', motor_names);
    set(gca, 'YTick', 1:4, 'YTickLabel', leg_names);
    title('扭矩-速度相关系数矩阵', 'FontSize', 13, 'FontWeight', 'bold');
    xlabel('电机', 'FontSize', 11);
    ylabel('腿部', 'FontSize', 11);
    
    % 在每个格子中显示数值
    for i = 1:4
        for j = 1:4
            text(j, i, sprintf('%.2f', corr_data(i,j)), ...
                 'HorizontalAlignment', 'center', ...
                 'FontSize', 10, 'FontWeight', 'bold', ...
                 'Color', 'white');
        end
    end
    
    % 绘制直方图
    subplot(2, 2, 3);
    histogram(torques, 50, 'FaceColor', [0.2 0.6 0.8]);
    xlabel('扭矩 (Nm)', 'FontSize', 10);
    ylabel('频数', 'FontSize', 10);
    title('扭矩分布直方图', 'FontSize', 11, 'FontWeight', 'bold');
    grid on;
    
    subplot(2, 2, 4);
    histogram(speeds, 50, 'FaceColor', [0.8 0.4 0.2]);
    xlabel('速度 (rad/s)', 'FontSize', 10);
    ylabel('频数', 'FontSize', 10);
    title('速度分布直方图', 'FontSize', 11, 'FontWeight', 'bold');
    grid on;
    
    %% 统计分析
    fprintf('\n========================================\n');
    fprintf('统计分析结果\n');
    fprintf('========================================\n');
    
    % 按腿部统计
    fprintf('\n【按腿部统计】\n');
    fprintf('%-8s %8s %12s %12s %12s %12s\n', ...
            '腿部', '数据点', '扭矩均值', '扭矩标准差', '速度均值', '速度标准差');
    fprintf('%s\n', repmat('-', 1, 70));
    
    for leg_id = 0:3
        mask = (leg_ids == leg_id);
        if sum(mask) > 0
            fprintf('%-8s %8d %12.3f %12.3f %12.3f %12.3f\n', ...
                    leg_names{leg_id+1}, sum(mask), ...
                    mean(torques(mask)), std(torques(mask)), ...
                    mean(speeds(mask)), std(speeds(mask)));
        end
    end
    
    % 按电机统计
    fprintf('\n【按电机统计】\n');
    fprintf('%-8s %8s %12s %12s %12s %12s\n', ...
            '电机', '数据点', '扭矩均值', '扭矩标准差', '速度均值', '速度标准差');
    fprintf('%s\n', repmat('-', 1, 70));
    
    for motor_id = 0:3
        mask = (motor_ids == motor_id);
        if sum(mask) > 0
            fprintf('%-8s %8d %12.3f %12.3f %12.3f %12.3f\n', ...
                    motor_names{motor_id+1}, sum(mask), ...
                    mean(torques(mask)), std(torques(mask)), ...
                    mean(speeds(mask)), std(speeds(mask)));
        end
    end
    
    % 全局统计
    fprintf('\n【全局统计】\n');
    fprintf('  总数据点数: %d\n', length(torques));
    fprintf('  扭矩统计:\n');
    fprintf('    均值:     %.3f Nm\n', mean(torques));
    fprintf('    标准差:   %.3f Nm\n', std(torques));
    fprintf('    最小值:   %.3f Nm\n', min(torques));
    fprintf('    最大值:   %.3f Nm\n', max(torques));
    fprintf('    范围:     %.3f Nm\n', max(torques) - min(torques));
    fprintf('  速度统计:\n');
    fprintf('    均值:     %.3f rad/s\n', mean(speeds));
    fprintf('    标准差:   %.3f rad/s\n', std(speeds));
    fprintf('    最小值:   %.3f rad/s\n', min(speeds));
    fprintf('    最大值:   %.3f rad/s\n', max(speeds));
    fprintf('    范围:     %.3f rad/s\n', max(speeds) - min(speeds));
    fprintf('  扭矩-速度总体相关系数: %.3f\n', corr(speeds, torques));
    
    %% 高级分析
    fprintf('\n【高级分析】\n');
    
    % 计算功率估计 (P = τ × ω)
    power = torques .* speeds;
    fprintf('  功率估计:\n');
    fprintf('    平均功率: %.3f W\n', mean(power));
    fprintf('    最大功率: %.3f W\n', max(power));
    fprintf('    最小功率: %.3f W\n', min(power));
    
    % 效率分析区域（假设理想工作区）
    ideal_speed_range = [mean(speeds) - std(speeds), mean(speeds) + std(speeds)];
    ideal_torque_range = [mean(torques) - std(torques), mean(torques) + std(torques)];
    in_ideal_zone = (speeds >= ideal_speed_range(1)) & (speeds <= ideal_speed_range(2)) & ...
                    (torques >= ideal_torque_range(1)) & (torques <= ideal_torque_range(2));
    fprintf('  理想工作区占比: %.1f%%\n', sum(in_ideal_zone) / length(torques) * 100);
    
    %% 导出统计报告
    try
        [filepath, name, ~] = fileparts(csv_filename);
        report_file = fullfile(filepath, [name '_analysis_report.txt']);
        
        fid = fopen(report_file, 'w');
        if fid ~= -1
            fprintf(fid, '四足机器人关节点云数据分析报告\n');
            fprintf(fid, '生成时间: %s\n', datestr(now));
            fprintf(fid, '数据文件: %s\n\n', csv_filename);
            fprintf(fid, '总数据点数: %d\n', length(torques));
            fprintf(fid, '扭矩均值: %.3f Nm, 标准差: %.3f Nm\n', mean(torques), std(torques));
            fprintf(fid, '速度均值: %.3f rad/s, 标准差: %.3f rad/s\n', mean(speeds), std(speeds));
            fprintf(fid, '相关系数: %.3f\n', corr(speeds, torques));
            fclose(fid);
            fprintf('  ✓ 统计报告已保存: %s\n', report_file);
        end
    catch
        fprintf('  ! 无法保存统计报告\n');
    end
    
    fprintf('\n========================================\n');
    fprintf('分析完成！已生成 %d 个图表窗口\n', 5);
    fprintf('========================================\n');
end

% 使用示例:
% 1. 直接指定文件: read_point_cloud_data('point_cloud_data_20251014_103045.csv')
% 2. 弹出文件选择对话框: read_point_cloud_data()
